/* USER CODE BEGIN Header */
/**
 ******************************************************************************
 * @file    can.c
 * @brief   This file provides code for the configuration
 *          of the CAN instances.
 ******************************************************************************
 * @attention
 *
 * Copyright (c) 2024 STMicroelectronics.
 * All rights reserved.
 *
 * This software is licensed under terms that can be found in the LICENSE file
 * in the root directory of this software component.
 * If no LICENSE file comes with this software, it is provided AS-IS.
 *
 ******************************************************************************
 */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "BSP_CAN.h"
#include "motor_control.h"
#include "control_config.h"
#include "main.h"
#include "encode_cali.h"

/* USER CODE BEGIN 0 */
#define MAX_Current 2500

void can_msg_handle()
{
    static uint8_t cmd_data[16] = {0};
    /* 数据包格式 ： 功能码 + 数据 + 校验码 */
    /* 回复包格式： 地址 + 功能码 + 命令状态 + 校验字节*/
    switch (bsp_can.RXData[0])
    {
    case C_CCUR:
        /* 设置校准电流 */
        /*拆包执行命令*/
        dec_param_reg[1] = ((bsp_can.RXData[1] << 8) | (bsp_can.RXData[2]));
        /*打包回复信息*/
        cmd_data[0] = XY_CAN_ID_ADD;
        cmd_data[1] = C_CCUR;
        cmd_data[2] = Work_Success;
        cmd_data[3] = 0x6B;
        /*回复*/
        can_send_msg(&cmd_data[0], 4);
        break;
    case C_RCUR:
        /* 设置额定电流 */
        dec_param_reg[0] = ((bsp_can.RXData[1] << 8) | (bsp_can.RXData[2]));
        /*打包回复信息*/
        cmd_data[0] = XY_CAN_ID_ADD;
        cmd_data[1] = C_RCUR;
        cmd_data[2] = Work_Success;
        cmd_data[3] = 0x6B;
        /*回复*/
        can_send_msg(&cmd_data[0], 4);
        break;
    case C_PID:
        /* 设置DCE_PID参数 */
        switch (bsp_can.RXData[1])
        {
            case DCE_KP:
                Control_DCE_SetKP((bsp_can.RXData[2] << 8) + (bsp_can.RXData[3]));
                cmd_data[2] = DCE_KP;
                break;
            case DCE_KI:
                Control_DCE_SetKI((bsp_can.RXData[2] << 8) + (bsp_can.RXData[3]));
                cmd_data[2] = DCE_KI;
                break;
            case DCE_KV:
                Control_DCE_SetKV((bsp_can.RXData[2] << 8) + (bsp_can.RXData[3]));
                cmd_data[2] = DCE_KV;
                break;
            case DCE_KD:
                Control_DCE_SetKD((bsp_can.RXData[2] << 8) + (bsp_can.RXData[3]));
                cmd_data[2] = DCE_KD;
                break;
            default:
                break;
        }
        cmd_data[0] = XY_CAN_ID_ADD;
        cmd_data[1] = C_PID;
        cmd_data[3] = Work_Success;
        cmd_data[4] = 0x6B;
        can_send_msg(&cmd_data[0], 5);
        break;
    case S_CPOS:
        /* 读取电机实时步数 */
        cmd_data[0] = XY_CAN_ID_ADD;
        cmd_data[1] = S_CPOS;
        cmd_data[2] = (uint8_t)(step_num >> 24);
        cmd_data[3] = (uint8_t)(step_num >> 16);
        cmd_data[4] = (uint8_t)(step_num >> 8);
        cmd_data[5] = (uint8_t)(step_num);
        cmd_data[6] = 0x6B;
        can_send_msg(&cmd_data[0], 7);

        break;
    // case RESET_CUR_POS_TO_ZERO:
    //     /* code */
    //     break;
    // case EN_CONTROL:
    //     /* code */
    //     break;
    // case SYNCHRONOUS_MOTION:
    //     /* code */
    //     break;
    
    default:
        break;
    }
}


/**
	* @brief   CAN1_RX0接收中断
	* @param   无
	* @retval  无
	*/
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
	// 接收一包数据
    if (HAL_CAN_GetRxFifoFillLevel(hcan, CAN_RX_FIFO0) == 0) /* 没有接收到数据 */
    {
        return;
    }

    if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &bsp_can.CanRx, &bsp_can.RXData[0]) != HAL_OK) /* 读取数据 */
    {
        return;
    }

    if (bsp_can.CanRx.IDE != CAN_ID_EXT || bsp_can.CanRx.RTR != CAN_RTR_DATA) /* 不是标准帧 / 不是数据帧 */
    {
        return;
    }

    //还要判定id是否匹配

	// 一帧数据接收完成，置位帧标志位
	bsp_can.rxflag = true;

    //协议解析
    can_msg_handle();
}
